#ifndef __KALMAN_H
#define __KALMAN_H

#include "motor.h"

// Define the structure for the Kalman filter
typedef struct 
{
    float P; //估算协方差
    float G; //卡尔曼增益
    float Q; //过程噪声协方差,Q增大，动态响应变快，收敛稳定性变坏
    float R; //测量噪声协方差,R增大，动态响应变慢，收敛稳定性变好
    float Output; //卡尔曼滤波器输出
	
} kalmanfilter;
   
extern kalmanfilter kf;

void KalmanInit(kalmanfilter* kf,float initial_P,float Q, float R, float initial_x);
float KalmanFilter(kalmanfilter *kf, float input);

#endif
